#include "handtracker.h"

using namespace cv;

HandTracker::HandTracker()
{
    model = cvCreateBGCodeBookModel();
    model->modMin[0] = 3;
    model->modMin[1] = model->modMin[2] = 3;
    model->modMax[0] = 10;
    model->modMax[1] = model->modMax[2] = 10;
    model->cbBounds[0] = model->cbBounds[1] = model->cbBounds[2] = 10;
    nframes = 0;
}

HandTracker::~HandTracker()
{
    delete model;
    delete yuvImage;
    delete ImaskCodeBook;
    delete ImaskCodeBookCC;
    delete model;
}


void HandTracker::detect ( Mat frame )
{
    ++nframes;
    IplImage rawImage = frame;
    IplImage blured;
    Mat tmpBlur;

    this->baseImage = frame;
    blur(this->baseImage,tmpBlur,Size(3,3));
    blured = tmpBlur;

    if (nframes == 1)
    {
        //CODEBOOK METHOD ALLOCATION
        yuvImage = cvCloneImage(&blured);
        ImaskCodeBook = cvCreateImage(cvGetSize(&blured),IPL_DEPTH_8U,1);
        ImaskCodeBookCC = cvCreateImage(cvGetSize(&blured),IPL_DEPTH_8U,1);
        cvSet(ImaskCodeBook,cvScalar(255));
    }

    cvCvtColor(&blured,yuvImage,CV_BGR2YCrCb);//YUV

    if (nframes-1 < nframesToLearnBG)
        cvBGCodeBookUpdate(model,yuvImage);

    if (nframes-1 == nframesToLearnBG)
        cvBGCodeBookClearStale(model,model->t/2);

    if (nframes-1>=nframesToLearnBG)
    {
        // Find foreground by codebook method
        cvBGCodeBookDiff( model, yuvImage, ImaskCodeBook );

        // This part just to visualize bounding boxes and centers if desired
        cvCopy(ImaskCodeBook,ImaskCodeBookCC);
        cvSegmentFGMask( ImaskCodeBookCC );
        this->intermediateImage = ImaskCodeBook;
        detect(ImaskCodeBookCC,frame);
    }

}

void HandTracker::detect(IplImage *ImaskCodeBookCC, Mat rawImage)
 {
     Mat drawing;
     rawImage.copyTo(drawing);

     int cptrDoigts = 0;
     //On crée les contours
     vector<vector<Point> > contours;
     findContours(cv::cvarrToMat(ImaskCodeBookCC),contours,CV_RETR_TREE,CV_CHAIN_APPROX_SIMPLE, Point(0, 0));

     //On crée les enveloppes et les défauts
     vector<vector<int> > hullsI(contours.size());
     vector<vector<Point> > hullsP(contours.size());
     vector<vector<Vec4i> > defects(contours.size());

     for (uint i=0; i<contours.size();i++)
     {
         convexHull(contours[i],hullsI[i],false);
         convexHull(contours[i],hullsP[i],false,true);
         if (hullsI[i].size()>3)
            convexityDefects(contours[i],hullsI[i],defects[i]);
     }

     for (uint i=0; i< contours.size();i++)
     {

         //http://docs.opencv.org/modules/imgproc/doc/structural_analysis_and_shape_descriptors.html
         for (uint j=0; j < defects[i].size();j++)
         {
             Vec4i tmp = defects[i][j]; //{startIndex,endIndex,farIndex,depth}
             Point start = contours[i][tmp[0]];
             Point end = contours[i][tmp[1]];
             Point far = contours[i][tmp[2]];
             float depth = tmp[3]/256.0;

             if (depth > 50)
             {
                line(drawing,start,far,Scalar(0,255,0));
                line(drawing,end,far,Scalar(0,255,0));
                circle(drawing,far,3,Scalar(255,0,0));
                circle(drawing,end,3,Scalar(0,0,255));
                cptrDoigts++;
             }
             else
             {
                 line(drawing,start,far,Scalar(0,255,0));
                 line(drawing,end,far,Scalar(0,255,0));
             }

         }
     }


     char txt[]="0";
     txt[0]='0'+cptrDoigts;
     putText(drawing, txt, cvPoint(50, 50), CV_FONT_HERSHEY_SIMPLEX, 1,Scalar(0, 0, 255));

     this->endImage = drawing;
 }
